/*
 *  main.c
 *  
 *
 *  Created by Philip Asare on 12/6/10.
 *  Copyright 2010 __MyCompanyName__. All rights reserved.
 *
 */

#include <nrk.h>
#include <include.h>
#include <ulib.h>
#include <stdio.h>
#include <avr/sleep.h>
#include <hal.h>
#include <rt_link.h>
#include <nrk_error.h>
#include <nrk_timer.h>
#include <nrk_ext_int.h>
#include <nrk_driver_list.h>
#include <nrk_driver.h>
#include <ff_basic_sensor.h>

NRK_STK MainStack[NRK_APP_STACKSIZE];
nrk_task_type MainTask;
void main_task(void);

/* controller actions messages */
#define UP 1
#define DOWN 2
#define LEFT 3
#define RIGHT 4
#define DRAW 5
#define ERASE 6

/* pin functions */
#define DRAW_BUTTON NRK_DEBUG_2
#define ERASE_BUTTON NRK_DEBUG_3
#define ACK NRK_UART1_TXD
#define IRQ NRK_UART0_TXD
#define BIT0 NRK_DEBUG_1
#define BIT1 NRK_UART1_RXD
#define BIT2 NRK_DEBUG_0
#define BIT3  NRK_UART0_RXD

/* interrupts */
#define DRAW_INT NRK_EXT_INT_0
#define ERASE_INT NRK_EXT_INT_1
#define ACK_INT EXT_INT_2



#define DEVIATION 10
uint16_t z_baseline, x_baseline, y_baseline;

/* controller tx slots --> base station rx slots */
#define C1

#ifdef C1
#define MY_TX 2
#define MY_RX 5
#endif

#ifdef C2
#define MY_TX 4
#define MY_RX 7
#endif

#ifdef C3
#define MY_TX 6
#define MY_RX 9
#endif

#ifdef C4
#define MY_TX 8
#define MY_RX 11
#endif

#define ALL_RX 3

/* game states and controller tracking */

uint8_t state, player_num, message;

#define SYNC 0
#define SELECT 1
#define PREGAME 2
#define GAMEPLAY 3
#define PLAYERASSIGN 4
#define GAMEOVER_CALC 5
#define GAMEOVER_CHOICE 6
#define CHANGE_STATE 7


/* monome helper functions */

#define type(x) (sizeof(x) + PKT_DATA_START)

typedef struct{
	uint8_t player;
} monome_comm_sync_pkt;

monome_comm_sync_pkt sync_pkt_tx, sync_pkt_rx;

/* monome functions */
uint8_t calibration;

uint8_t monome_set_player_num();
inline void monome_setup_system();
inline void monome_comm_init();
inline void monome_comm_set_msg(uint8_t msg);
inline void monome_comm_send_msg();
inline void monome_move(uint8_t direction);
void monome_comm_rcv_ack();
void monome_draw();
void monome_erase();

uint8_t tx_len;

/* task setup */

NRK_STK CommStack[NRK_APP_STACKSIZE];
nrk_task_type CommTask;
void comm_task(void);

void nrk_create_taskset();

uint8_t tx_buf[RF_MAX_PAYLOAD_SIZE];
uint8_t rx_buf[RF_MAX_PAYLOAD_SIZE];

/* monome sketch-it functions */

void nrk_register_drivers();
uint8_t kill_stack(uint8_t val);

int
main ()
{
	uint8_t t;
	nrk_setup_ports();
	//nrk_setup_uart(UART_BAUDRATE_115K2);
	
	/* output pins */
	nrk_gpio_direction(BIT0, NRK_PIN_OUTPUT);
	nrk_gpio_direction(BIT1, NRK_PIN_OUTPUT);
	nrk_gpio_direction(BIT2, NRK_PIN_OUTPUT);
	nrk_gpio_direction(BIT3, NRK_PIN_OUTPUT);
	nrk_gpio_direction(IRQ, NRK_PIN_OUTPUT);
	
	/* button input pins */
	nrk_gpio_direction(DRAW_BUTTON, NRK_PIN_INPUT);
	nrk_gpio_direction(ERASE_BUTTON, NRK_PIN_INPUT);
	nrk_gpio_direction(ACK, NRK_PIN_INPUT);
	
	/* configure interrupts */
	nrk_ext_int_configure(DRAW_INT, NRK_RISING_EDGE, monome_draw);
	nrk_ext_int_configure(ERASE_INT, NRK_RISING_EDGE, monome_erase);
	
	nrk_gpio_clr(IRQ);
	
	
	//printf( PSTR("starting...\r\n") );
	
	nrk_init();
	nrk_time_set(0,0);
	
	nrk_register_drivers();
	nrk_create_taskset ();
	nrk_start();
	
	return 0;
}

void main_task()
{
	uint16_t cnt;
	int8_t i,fd,val;
	uint16_t x_val, y_val, z_val;
	
	//printf( "My node's address is %d\r\n",NODE_ADDR );
	
	//printf( "Task1 PID=%d\r\n",nrk_get_pid());
	
	nrk_ext_int_enable(DRAW_INT);
	nrk_ext_int_enable(ERASE_INT);
	
	x_baseline = 0;
	y_baseline = 0;
	z_baseline = 0;
	cnt=0;
	calibration = 1;
	monome_setup_system();
	//monome_comm_init();
	
	/*while(!rtl_ready())  {
		nrk_led_toggle(RED_LED);
		nrk_wait_until_next_period(); 
	}*/
	while(1) {
		// Open ADC device as read 
		fd=nrk_open(FIREFLY_SENSOR_BASIC,READ);
		if(fd==NRK_ERROR) nrk_kprintf(PSTR("Failed to open sensor driver\r\n"));
		
		/* read x value */
		val=nrk_set_status(fd,SENSOR_SELECT,ACC_X);
		val=nrk_read(fd,&x_val,2);
		
		/* read y value */
		val=nrk_set_status(fd,SENSOR_SELECT,ACC_Y);
		val=nrk_read(fd,&y_val,2);
		
		/* read z value  */
		val=nrk_set_status(fd,SENSOR_SELECT,ACC_Z);
		val=nrk_read(fd,&z_val,2);
		
		if(cnt > 10){
			calibration = 0;
			//printf( "z_baseline=%d\r\n",z_baseline);
			//printf( "x_baseline=%d\r\n",x_baseline);
			//printf( "y_baseline=%d\r\n",y_baseline);
			cnt = 0;
		}
		nrk_led_toggle(GREEN_LED);
		/* calibration */
		if(calibration > 0){
			nrk_led_toggle(BLUE_LED);
			if(cnt == 0){
				x_baseline =  x_val;
				y_baseline =  y_val;
				z_baseline =  z_val;
			}
			else{
				x_baseline = (x_baseline + x_val)>>1;
				y_baseline = (y_baseline + y_val)>>1;
				z_baseline = (z_baseline + z_val)>>1;
			}
			
			cnt++;
			//nrk_led_clr(BLUE_LED);
		}
		
		else{	
			//if(state == SELECT || state == GAMEPLAY || state == PREGAME || state == GAMEOVER_CHOICE){
				/* 
				 * respond to accelerometer only if you are in the appropriate state
				 * change in z value beyond threshold indicates cursor move
				 * determine cursor direction only on this indicator
				 */
				if((z_val > z_baseline + DEVIATION) || z_val < z_baseline - DEVIATION){
					
					//printf( "acc_z=%d",z_val);
					//printf( " acc_x=%d",x_val);
					//printf( " acc_y=%d\r\n",y_val);
					if((x_val > (x_baseline + DEVIATION)) || (x_val  < (x_baseline - DEVIATION))){
						if(x_val > x_baseline) //nrk_kprintf( PSTR("move down\r\n") );
							monome_move(DOWN);	
						else //nrk_kprintf( PSTR("move up\r\n") );
							monome_move(UP);
					}
					if((y_val > (y_baseline + DEVIATION)) || (y_val  < (y_baseline - DEVIATION))){
						if(y_val > y_baseline) //nrk_kprintf( PSTR("move right\r\n") );
							monome_move(RIGHT);
						else //nrk_kprintf( PSTR("move left\r\n") );
							monome_move(LEFT);
					}
				}
				else {
					//nrk_kprintf( PSTR("no move\r\n") );
					;
				}
			//}
			
		}
		
		nrk_close(fd);
		nrk_wait_until_next_period();
		
	}
}


/*void comm_task()
{
	//uint8_t j, i;
	uint8_t *local_rx_buf;
	uint8_t length,slot;
	int8_t rssi;
	
	
	monome_setup_system();
	
	while(!rtl_ready())  nrk_wait_until_next_period(); 
	
	while(1){
		// start communication after calibration 
		if(calibration == 0){
			if( rtl_rx_pkt_check()!=0 )
			{
				//if packet is received
				local_rx_buf=rtl_rx_pkt_get(&length, &rssi, &slot);
				printf( "Got Packet on slot %d %d: \r\n",slot,length );
				if(length == type(monome_comm_sync_pkt)){
					// if in sync state, set the player number and switch to select state
					if(state == SYNC){
						player_num = local_rx_buf[PKT_DATA_START];
						rtl_rx_pkt_release();
						if(monome_set_player_num()){
							monome_comm_set_msg(CHANGE_STATE);
							monome_comm_send_msg();
							monome_comm_set_msg(player_num);
							monome_comm_send_msg();
						}
						
						
					}
					//if in any other state, then message is state information, change state and alert HCS12
					else{
						state = local_rx_buf[PKT_DATA_START];
						rtl_rx_pkt_release();
						monome_comm_set_msg(CHANGE_STATE);
						monome_comm_send_msg();
						monome_comm_set_msg(state);
						monome_comm_send_msg();
					}
				}
			}
			if(state == SYNC){
				//if in sync state, send sync packet to base station
				printf("waiting to sync\r\n");
				while( rtl_tx_pkt_check(MY_TX)!=0 );
				memcpy(&tx_buf[PKT_DATA_START], &sync_pkt_tx, sizeof(monome_comm_sync_pkt));
				tx_len = type(monome_comm_sync_pkt);
				rtl_tx_pkt( tx_buf, tx_len, MY_TX);
				rtl_wait_until_tx_done(MY_TX);
			}
		}
		
		nrk_wait_until_next_period();
	}
}
*/
void
nrk_create_taskset()
{
	
/*	
	CommTask.task = comm_task;
	CommTask.Ptos = (void *) &CommStack[NRK_APP_STACKSIZE-1];
	CommTask.Pbos = (void *) &CommStack[0];
	CommTask.prio = 2;
	CommTask.FirstActivation = TRUE;
	CommTask.Type = BASIC_TASK;
	CommTask.SchType = PREEMPTIVE;
	CommTask.period.secs = 0;
	CommTask.period.nano_secs = 500*NANOS_PER_MS;
	CommTask.cpu_reserve.secs = 0;
	CommTask.cpu_reserve.nano_secs = 200*NANOS_PER_MS;
	CommTask.offset.secs = 0;
	CommTask.offset.nano_secs= 0;
	nrk_activate_task (&CommTask);
*/	
	MainTask.task = main_task;
	nrk_task_set_stk( &MainTask, MainStack, NRK_APP_STACKSIZE);
	MainTask.prio = 1;
	MainTask.FirstActivation = TRUE;
	MainTask.Type = BASIC_TASK;
	MainTask.SchType = PREEMPTIVE;
	MainTask.period.secs = 0;
	MainTask.period.nano_secs = 500*NANOS_PER_MS; //*NANOS_PER_MS;
	MainTask.cpu_reserve.secs = 0;
	MainTask.cpu_reserve.nano_secs =  200*NANOS_PER_MS;
	MainTask.offset.secs = 0;
	MainTask.offset.nano_secs= 0;
	nrk_activate_task (&MainTask);
	
	
	printf ("Create done\r\n");
}

inline void monome_comm_init(){
	rtl_init (RTL_MOBILE);
	
	/* rx schedules */
	rtl_set_schedule( RTL_RX, MY_RX, 1 );
	rtl_set_schedule( RTL_RX, ALL_RX, 1 );
	
	/* tx schedules */
	rtl_set_schedule( RTL_TX, MY_TX, 1 );
	
	rtl_start();
	rtl_rx_pkt_set_buffer(rx_buf, RF_MAX_PAYLOAD_SIZE);
	
	// clear communication port
	nrk_gpio_clr(BIT0);
	nrk_gpio_clr(BIT1);
	nrk_gpio_clr(BIT2);
	nrk_gpio_clr(BIT3);
	nrk_gpio_clr(IRQ);
}

inline void monome_setup_system(){
	state = SYNC;
	player_num = -1;
	//sync_pkt_tx.player = -1;
	// clear communication port
	nrk_gpio_clr(BIT0);
	nrk_gpio_clr(BIT1);
	nrk_gpio_clr(BIT2);
	nrk_gpio_clr(BIT3);
	nrk_gpio_clr(IRQ);
}

uint8_t monome_set_player_num(){
	printf("setting player\r\n");
	//player_num = sync_pkt_rx.player;
	printf("player number: %d\r\n", player_num);
	switch (player_num) {
		case 1:
			nrk_led_set(RED_LED);state = SELECT; return 1;

		case 2:
			nrk_led_set(GREEN_LED);state = SELECT;  return 1;

		case 3:
			nrk_led_set(RED_LED); nrk_led_set(GREEN_LED);state = SELECT;  return 1;

		case 4:
			nrk_led_set(BLUE_LED);state = SELECT;  return 1;

		default:
			 return 0;
	}
	
}

void nrk_register_drivers()
{
	int8_t val;
	
	// Register the Basic FireFly Sensor device driver
	// Make sure to add: 
	//     #define NRK_MAX_DRIVER_CNT  
	//     in nrk_cfg.h
	// Make sure to add: 
	//     SRC += $(ROOT_DIR)/src/drivers/platform/$(PLATFORM_TYPE)/source/ff_basic_sensor.c
	//     in makefile
	val=nrk_register_driver( &dev_manager_ff_sensors,FIREFLY_SENSOR_BASIC);
	if(val==NRK_ERROR) nrk_kprintf( PSTR("Failed to load my ADC driver\r\n") );
	
}
inline void monome_comm_set_msg(uint8_t msg){
	if(msg & 0x01){
		nrk_gpio_set(BIT0);
	}
	else{
		nrk_gpio_clr(BIT0);
	}
	if(msg & 0x02){
		nrk_gpio_set(BIT1);
	}
	else{
		nrk_gpio_clr(BIT1);
	}
	if(msg & 0x04){
		nrk_gpio_set(BIT2);
	}
	else{
		nrk_gpio_clr(BIT2);
	}
	if(msg & 0x08){
		nrk_gpio_set(BIT3);
	}
	else{
		nrk_gpio_clr(BIT3);
	}
}
inline void monome_comm_send_msg(){
	nrk_led_set(RED_LED);
	nrk_gpio_set(IRQ);
	nrk_spin_wait_us(1000); /**TO-DO: experiment to see how quick a change HCS12 can handle */
	nrk_gpio_clr(IRQ);
	nrk_led_clr(RED_LED);
}

void monome_move(uint8_t direction){
	monome_comm_set_msg(direction);
	monome_comm_send_msg();
}

void monome_draw(){
	while(nrk_gpio_get(DRAW_BUTTON)); //debounce
	//if(state == SELECT || state == GAMEPLAY || state == PREGAME || state == GAMEOVER_CHOICE){
		// respond to controls only if in appropriate state
		monome_comm_set_msg(DRAW);
		monome_comm_send_msg();
	//}//
}

void monome_erase(){
	while(nrk_gpio_get(ERASE_BUTTON)); //debounce
	//if(state == SELECT || state == GAMEPLAY || state == PREGAME || state == GAMEOVER_CHOICE){
		// respond to controls only if in appropriate state
		monome_comm_set_msg(ERASE);
		monome_comm_send_msg();
	//}
}
